#include "power_board_pkg/base_driver.h"
#include "power_board_pkg/data_holder.h"
#include "power_board_pkg/simple_dataframe_master.h"
#include "power_board_pkg/timer_ros.h"


#define MAXXSPEED 0.5
#define MAXWSPEED 0.5

BaseDriver* BaseDriver::instance = NULL;
rclcpp::Node::SharedPtr BaseDriver::node= NULL;

typedef void (*TaskFuc)(BaseDriver*);

double g_Tasktime[20];

typedef struct
{
  double* tasktime;
  double timeout;
  TaskFuc taskfuc;
}t_TaskType;

t_TaskType g_TaskArray[] = 
{
    {&g_Tasktime[0],500,&BaseDriver::Get_PB_Status},
    {&g_Tasktime[1],1000,&BaseDriver::Get_Battery_Data},
    {&g_Tasktime[2],1000,&BaseDriver::Get_Btn_Data},
    {&g_Tasktime[3],10,&BaseDriver::Get_Charge_Status},
    {&g_Tasktime[4],249,&BaseDriver::Send_Heart_Beat},  //yyy 2020-9-18 v3.0.4
    {&g_Tasktime[5],2000,&BaseDriver::Read_Pb_Version},  //yyy 2020-10.13 v3.1.2
    // {&g_Tasktime[6],100,&BaseDriver::Check_Limit_Power},  //检测极限电源 yyy 2020-10.14 v3.1.2
};


BaseDriver::BaseDriver()
{
    Base_Driver_Param_Get();
    g_Mb_Status = 0;
 
}

BaseDriver::~BaseDriver()
{
    if (instance != NULL)
        delete instance;
}


void BaseDriver::init_ros_topic()
{
	pb_status_pub = node->create_publisher<std_msgs::msg::UInt32>("pb_status", 1);
    battery_data_pub = node->create_publisher<ubt_interface::msg::BatteryData>("BatteryData", 1);
	charge_status_pub = node->create_publisher<std_msgs::msg::UInt8>("pb_charge_status",1);
    lift_cmd_pub = node->create_publisher<std_msgs::msg::Int8>("lift_motor_cmd_vel",1);
    pb_version_pub = node->create_publisher<std_msgs::msg::String>("pb_version",10);
    power_state_pub = node->create_publisher<std_msgs::msg::String>("power_state",1);


    ledbar_order_sub = node->create_subscription<std_msgs::msg::UInt8MultiArray>("LedbarOrder",10, std::bind(&BaseDriver::LedbarOrder_Callback, this, std::placeholders::_1));
    mb_status_sub = node->create_subscription<std_msgs::msg::UInt32>("mb_status",1,std::bind(&BaseDriver::Mb_Status_Callback, this, std::placeholders::_1));
    ubt_state_o_obstacle_alarm_sub = node->create_subscription<std_msgs::msg::String>("ubt_state_o_obstacle_alarm",1,std::bind(&BaseDriver::Ubt_State_o_Obstacle_Alarm_Callback, this, std::placeholders::_1));
}

void BaseDriver::work_loop()
{
    rclcpp::Rate loop(1000);


    for(int i = 0;i < sizeof(g_TaskArray)/sizeof(t_TaskType);i ++)
    {
        g_Tasktime[i] = Get_Curtime();
    }

    while (rclcpp::ok())
    {
    	for(int i = 0;i < sizeof(g_TaskArray)/sizeof(t_TaskType);i ++)
    	{
			if(is_Timeout(g_TaskArray[i].tasktime,g_TaskArray[i].timeout))
			{
                g_TaskArray[i].taskfuc(BaseDriver::Instance());
			}
    	}
		rclcpp::spin_some(node);
        loop.sleep();
    }
}

/***************************************timer funtion************************************************/


void BaseDriver::Read_Pb_Version(BaseDriver* base)
{
//    static int count = 0;
//    if(count < 10)  //连续发送10次查询版本号，然后就不发送了，避免开机只查询一次会遗漏，也避免过多查询造成冗余
//    {
//        count ++;
	Simple_dataframe::Frame_Instance()->interact(SET_GET_FIRMWARE_VERSION,NULL,0); //yyy 2020-10-13 v3.1.2
//    }
}
/*心跳发送*/
//yyy 2020-9-18 v3.0.4
void BaseDriver::Send_Heart_Beat(BaseDriver* base)
{
	Simple_dataframe::Frame_Instance()->interact(SET_HEART_BEAT,NULL,0);
	// Simple_dataframe::Frame_Instance()->interact(SET_GET_FIRMWARE_VERSION,NULL,0);
}

/*更新电池的充电状态 0-未充电；1-充电中；2-充电异常*/
void BaseDriver::Get_Charge_Status(BaseDriver* base)
{
    Data_holder* dh = Data_holder::get();

	if(dh->Flag._chargestatusupdate == true)
	{
		std_msgs::msg::UInt8 chargestatus;
		chargestatus.data = dh->batterydata.chargestatus;
		base->charge_status_pub->publish(chargestatus);
		dh->Flag._chargestatusupdate = false;
	}
}


/*更新电源板状态字到ros数据处理节点*/
void BaseDriver::Get_PB_Status(BaseDriver* base)
{
    std_msgs::msg::UInt32 agvstatus;//agv状态
    agvstatus.data = Data_holder::get()->power_board.status;
    // printf("Data_holder::get()->power_board.status = %d\n",Data_holder::get()->power_board.status);
    // printf(" agvstatus.data = %d\n", agvstatus.data);
    base->pb_status_pub->publish(agvstatus);
}


/*更新电池信息到ros节点*/
void BaseDriver::Get_Battery_Data(BaseDriver* base)
{
    Data_holder* dh = Data_holder::get();
    ubt_interface::msg::BatteryData batterydata;
    static uint8_t limitcount = 0;
    static uint8_t lowercount = 0;
    // Simple_dataframe::Frame_Instance()->interact(SET_GET_FIRMWARE_VERSION,NULL,0);

    if(dh->Flag._batteryupdate == true)
    {
        batterydata.curpower = dh->batteryinfo.totalvol;
		batterydata.perpower = dh->batteryinfo.soc;
		batterydata.chargestatus = dh->batteryinfo.bmsstatus;
        base->battery_data_pub->publish(batterydata);
        // RCLCPP_INFO(node->get_logger(),"cur soc = %d\n",dh->batteryinfo.soc);
        dh->Flag._batteryupdate = false;

        if(batterydata.perpower <= base->g_Lower_Power)  //低电量 yyy 2020-10-31 v3.2
        {
        	if(lowercount > 5)
    		{
    			lowercount = 0;
				base->Power_state.data = "LOWER";
           		base->power_state_pub->publish(base->Power_state);
    		}
			else
			{
				lowercount ++;
			}
    
        }
        else  //电量高于低电量阈值
        {
            base->Power_state.data = "ENOUGH";
            base->power_state_pub->publish(base->Power_state);
        }
        if(batterydata.perpower <= base->g_Limit_Power)  //极限电量 yyy 2020-11-25 v3.3
        {
            if(limitcount > 10)  //防止极限电量的时候，电量会有波动
            {
                limitcount = 0;
                base->Power_state.data = "LIMIT";
                base->power_state_pub->publish(base->Power_state);
            }
            else
            {
                limitcount ++;
            }
        }
    }
}

void BaseDriver::Get_Btn_Data(BaseDriver* base)
{
    Data_holder* dh = Data_holder::get();
    std_msgs::msg::Int8 liftcmd;
    if(dh->Flag._btndataupdate == true)
    {
        if(dh->power_board.btn == 0x02)
        {
            if(base->g_Mb_Status & BIT(6))
                liftcmd.data = -1;
            else if(base->g_Mb_Status & BIT(7))
                liftcmd.data = 1;
            else
                liftcmd.data = -1;
            base->lift_cmd_pub->publish(liftcmd);
        }
  
       dh->Flag._btndataupdate = false;
    }
}

/**********************************topic sub callback***************************************/
void BaseDriver::Ubt_State_o_Obstacle_Alarm_Callback(const std_msgs::msg::String::SharedPtr alarm)
{   
    Obstacle_Alarm = 1;
}
void BaseDriver::Mb_Status_Callback(const std_msgs::msg::UInt32::SharedPtr status)
{
    g_Mb_Status = status->data;
}

/*led灯条控制命令*/
void BaseDriver::LedbarOrder_Callback(const std_msgs::msg::UInt8MultiArray::SharedPtr rdata)
{
	uint8_t ledorder[5] = {0};
	uint16_t lednum;
    for(int i = 0;i < 5;i++)
    {
        ledorder[i] = rdata->data.at(i+1);
    }

    lednum = (0x400 | rdata->data.at(0));
    // printf("lednum = %x\n",lednum);
    Simple_dataframe::Frame_Instance()->interact((MESSAGE_ID)lednum,ledorder,5);
}

/*获取系统参数*/
void BaseDriver::Base_Driver_Param_Get(void)
{
	g_Limit_Power = node->get_parameter("/power_board/limitpower").as_int();
    node->declare_parameter("/power_board/lowerpower",20);
    g_Lower_Power = node->get_parameter("/power_board/lowerpower").as_int();
}



